# -*- coding: utf-8 -*-
__author__ = 'xujianhua'
import sys,os
sys.path.append(os.path.dirname(sys.path[0]))
import numpy as np
import cv2
import time
from cv2 import *
from lcopa import calculate_codev,load_uphi_data,lcopa_usb



def parse_pic(queue):
    cap = cv2.VideoCapture(1)
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,1280);
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,720);
    cap.set(cv2.cv.CV_CAP_PROP_FPS,1);
    cal_frame = 0
    frame_err = 0.0
    toal_angle = 0.0
    MAX_FRAME = 10
    last_ave = 0
    # lcopausb = lcopa_usb.LcopaUsb()
    # lcopausb.open(0x1234)
    # mu_phi = load_uphi_data.u_phi("./u-phi/DF2015-FA02.txt")
    # print mu_phi.toString()
    # lcoap = calculate_codev.lcopa_vol(mu_phi)
    # btyes=lcoap.calculate_voltage((0.003))
    # lcopausb.send_byte(btyes)
    # bp = bp_nn.BPNN('','')
    # str_time = time.strftime('%Y-%m-%d-%H-%M-%S',time.localtime(time.time()))
    # fobj=open("./"+str_time+".txt",'w')
    while(True):
        # Capture frame-by-frame
        ret, frame = cap.read()
        gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)

        kernel = np.ones((5,5),np.float32)/25
        filter_frame = cv2.medianBlur(gray,31)#cv2.filter2D(dst_img,-1,kernel)  #41
        #cv2.imshow('filter_frame',filter_frame)
        hist = np.average(filter_frame.ravel())
        _,out=cv2.threshold(filter_frame,240,255,cv2.THRESH_BINARY)
        cv2.imshow('out1',out)
        # # cv2.imshow('out1',out)
        contours, hierarchy = cv2.findContours(out,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
        sumx=0
        sumy=0
        area=0
        max_area = 0
        max_x = 0
        max_y = 0
        
        last_ave_rin = 0.0
        for i in range(0,len(contours)):
            area = cv2.contourArea(contours[i])
            if area >  max_area:
                center,r = cv2.minEnclosingCircle(contours[i])
                max_area = area
                frame_err += (float(center[0]))
                #print 'center,r',center,r
                if(cal_frame == MAX_FRAME):
                    cal_frame = 0
                    print "x:",(frame_err/MAX_FRAME)
                    # ave_rin = ((frame_err/MAX_FRAME)-640)*0.00001
                    # queue.put(ave_rin)
                    # (kp,ki,kd,wi,wo,u) = bp.sim((frame_err/MAX_FRAME)*0.00001,toal_angle,ave_rin)
                    # fobj.write(("pid:%f,%f,%f--%f\n")%(kp,ki,kd,(frame_err/MAX_FRAME)))
                    # fobj.write(str(wi))
                    # fobj.write('\n')
                    # fobj.write(str(wo))
                    # fobj.write('\n')
                    # last_ave_rin = ave_rin
                    # print 'pid:',kp,ki,kd
                    # toal_angle = u
                    # print "toal_angle",toal_angle
                    # #print 'ave_rin',ave_rin
                    # #print 'u',u
                    #         #print 'toal_angle:',toal_angle
                    frame_err = 0
                    # btyes=lcoap.calculate_voltage_pi(toal_angle,6.2831653)
                    # lcopausb.send_byte(btyes)
                else:
                    cal_frame += 1
            if max_area != 0:
                cv2.drawContours(frame,contours,-1,255)
                #cv2.circle(frame,(int(center[0]),int(center[1])),int(r),255,-1)
                #print "x-y",(sumy/area,sumx/area)
        cv2.imshow('image',frame)
        #cv2.imshow('filter_frame',filter_frame)
        #cv2.imshow('result frame',result)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            # fobj.flush()
            # fobj.close()
            break
    cap.release()
    cv2.destroyAllWindows()
